/*
 * FeedbackTask.c
 *
 *  Created on: 10-07-2012
 *      Author: Mati
 */

#include "ApplicationTasks.h"
#define OUT_VOLTAGE(x) 			(float)((9.3284*(float)(x))-(float)70.492)	//wzmocnienie wzmacniacza
#define OUT_VOLTAGE_LOW(x) 		(float)((0.0249*((float)(x)*(float)(x)))-((float)1.0132*(float)(x))+(float)599.51)
#define RES_POM_VOLTAGE(x) 		(float)(0.035*((x))-(float)0.0042)
#define RES_POM_VOLTAGE_LOW(x) 	(float)(0.0295*((x))+(float)0.7417)

enum Process eProcess = COOLING;

uint32_t ADC_Meas[NUM_OF_ADC_SAMPLES];
uint32_t ADC_Meas_Idx = 0;



float precVoltage = (float)1070;	//V
extern uint16_t DesiredTemperature;

void vFeedbackTask(void *pvParameters){
	//portCHAR cIn;
	float val;
	//uint16_t u16val;
	float precResistance;
	float adc_voltage;
	uint32_t ptrAdcVoltage = (uint32_t)&adc_voltage;
	float mCurrent;
	float mVoltage;
	float precTemperature;

	uint8_t filter_cnt = 0;
	uint8_t filter_v = 0;
	float lastADC = 0;
	float lastDAC;
	float lastRes = 132;
	float minTemp = 0;
	float maxTemp = 0;
	float maxADC = 0;
	float minADC = 0;
	//float stimulatedTemperature = 0;

	NVIC_EnableIRQ(RIT_IRQn);
	vSemaphoreCreateBinary( xAdc2UartSem );


/*
#ifdef	INCLUDE_uxTaskGetStackHighWaterMark == 1
	unsigned portBASE_TYPE uxHighWaterMark;
    // Inspect our own high water mark on entering the task.
	uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
	printf("Feedback Task Stack: %d\n\r", uxHighWaterMark);
#endif
*/

	NVIC_EnableIRQ(TIMER1_IRQn);
	LPC_TIM1->IR |= 0xff; // Clear all timer interrupts if there are any
	LPC_TIM1->TCR |= 0x01; // start timer.


	xSemaphoreTake( xAdc2UartSem, 0 ); //pominiecie 1 odczytu

	while(1){

			if ( xAdc2UartSem != NULL ){
				if ( pdTRUE == xSemaphoreTake( xAdc2UartSem, (portTickType) portMAX_DELAY)){ //&& (1 == (LPC_ADC->ADSTAT & 0x20)>>5) ){
					//NVIC_DisableIRQ(TIMER1_IRQn);
					LPC_GPDMACH0->DMACCConfig &= ~1; 				//Enable Channel0
					//val = (uint32_t)((LPC_ADC->ADDR5>>4) & 0xFFF);

					if( DESORPTION == ewState )
					{
						eProcess = HEATING;
					}
					else if( ADSORPTION == ewState )
					{
						eProcess = COOLING;
					}
					val = AverageValue(ADC_Meas);

					if(PVal.dac_voltage > 70 && PVal.dac_voltage < 3200)
						precVoltage = OUT_VOLTAGE(PVal.dac_voltage);
					else if(PVal.dac_voltage >= 3200)
						precVoltage = 29640;
					else if(PVal.dac_voltage <= 70 && PVal.dac_voltage > 28)
						precVoltage = OUT_VOLTAGE_LOW(PVal.dac_voltage);
					else if(PVal.dac_voltage <= 28)
						precVoltage = 576;

					Send(PMarker.voltage, (uint8_t*) &precVoltage);

					adc_voltage = (float)((val*(float)3300)/(float)4095);	//max adc volt = 3300mV
					if(NaN == *((uint32_t*)ptrAdcVoltage) || adc_voltage>3300 || adc_voltage<0)
						adc_voltage = lastADC;
//					if( HEATING == eProcess )
//					{
//						if( 0 == lastADC )
//						{
//							//minADC = adc_voltage;
//							lastADC = adc_voltage;
//						}
//						if( !(adc_voltage < 2860 && adc_voltage > 2640) )
//							adc_voltage = lastADC - 0.001;
//						if( adc_voltage > lastADC + 10 || adc_voltage < lastADC - 10 )
//							adc_voltage = lastADC - 0.001;
////						if(adc_voltage < minADC)
////							minADC = adc_voltage;
////						else
////							adc_voltage = lastADC;
//					}
//					else if(COOLING == eProcess)
//					{
//						if( 0 == lastADC )
//						{
//							//maxADC = adc_voltage;
//							lastADC = adc_voltage;
//						}
//						if( !(adc_voltage < 95 && adc_voltage > 110) )
//							adc_voltage = lastADC - 0.1;
//						if( adc_voltage > lastADC + 5 || adc_voltage < lastADC - 5 )
//							adc_voltage = lastADC - 0.1;
////						if(adc_voltage > maxADC)
////							maxADC = adc_voltage;
////						else
////							adc_voltage = lastADC;
//					}

//					if(PVal.dac_voltage == lastDAC && (adc_voltage < lastADC-10 || adc_voltage > lastADC+10))
//					{
//						filter_cnt++;
//						if(filter_cnt > 1)
//						{
//							lastADC = adc_voltage;
//							filter_cnt = 0;
//						}
//						adc_voltage = lastADC;
//					}
//					else if( PVal.dac_voltage != lastDAC )
//					{
//						lastADC = adc_voltage;
//						filter_cnt = 0;
//					}

					////adc_voltage = adc_voltage/(float)4095;	//napiecie ADC w mV
					Send(PMarker.adc_voltage, (uint8_t*) &adc_voltage);
					//if(COOLING == eProcess)
					if(PVal.dac_voltage > 90 && PVal.dac_voltage < 3200)
						mVoltage = RES_POM_VOLTAGE(adc_voltage);
					//else if(PVal.dac_voltage >= 3200)
					//	mVoltage = 115.6;
					else if(PVal.dac_voltage <= 70 && PVal.dac_voltage > 28)
						mVoltage = OUT_VOLTAGE_LOW(PVal.dac_voltage);
					else if(PVal.dac_voltage <= 28)
						mVoltage = 576;

						mVoltage = RES_POM_VOLTAGE(adc_voltage);
					//else if(HEATING == eProcess)
						//mVoltage = RES_POM_VOLTAGE_UP(adc_voltage);//napiecie na rezystorze pomiarowym
					mCurrent = (mVoltage * (float)2); 						//3300mV - maks napięcie, *2 bo rezystor 0.5[Ohm] i /73,5 bo tyle wynosi wzmocnienie monitora pradowego

					//mCurrent = (float)(val*(float)235.925);//3300mV*2/27,975
					//mCurrent = mCurrent/(float)4095;
					Send(PMarker.current, (uint8_t*) &mCurrent);
					//mVoltage = (mCurrent/(float)2);
					precVoltage -= mVoltage;
					precResistance = (precVoltage)/mCurrent;			//rezystancja prekoncentratora
					//if((precResistance > lastRes + 10 || precResistance < lastRes - 10))
					//{
					//	precResistance = lastRes;
					//}

					Send(PMarker.resistance, (uint8_t*) &precResistance);
					//printf("Rezystancja prekoncentratora: %4.2f\n\r", precResistance);
					precTemperature = 19.111*precResistance - 2443.8;
//					if(HEATING == eProcess)
//					{
//						//precTemperature = (PVal.A_TWR * precResistance) + PVal.B_TWR;
//						precTemperature = 19.111*precResistance - 2443.8;
//						if(0 == maxTemp)
//							maxTemp = precTemperature;
//						if(precTemperature > maxTemp && precTemperature < maxTemp + 40)
//						{
//							maxTemp = precTemperature;
//						}
//						else
//						{
//							maxTemp += 0.01;
//							precTemperature = maxTemp;
//						}
//						minTemp = maxTemp;
//					}
//					else if(COOLING == eProcess)
//					{
//						precTemperature = 22.323*precResistance - 2737.8;
//						if(0 == minTemp)
//							minTemp = precTemperature;
//						if(precTemperature < minTemp && precTemperature > minTemp - 40)
//						{
//							minTemp = precTemperature;
//						}
//						else
//						{
//							minTemp -= 0.01;
//							precTemperature = minTemp;
//						}
//						maxTemp = minTemp;
//					}

					//printf("Temperatura odczytana: %3.2f\n\r", precTemperature);
					//precTemperature = 130.16;
					Send(PMarker.temperature, (uint8_t*) &precTemperature );

					//stimulatedTemperature = PIDcal( (float)DesiredTemperature, (float)(DesiredTemperature - stimulatedTemperature*0.2) );

					//printf("Temperatura podana: %4.2f\n\r", stimulatedTemperature);
					//zamkniecie petli sprzezenia
					ValueDAC( PVal.dac_voltage );//stimulatedTemperature );
					lastADC = adc_voltage;
					lastDAC = PVal.dac_voltage;
					lastRes = precResistance;
					SetUpNewDMATransferFromADC(0);

/*
					#ifdef	INCLUDE_uxTaskGetStackHighWaterMark == 1
					// Calling the function will have used some stack space, we would
					// therefore now expect uxTaskGetStackHighWaterMark() to return a
					// value lower than when it was called on entering the task.
						uxHighWaterMark = uxTaskGetStackHighWaterMark( NULL );
						printf("Feedback Task Stack: %d\n\r", uxHighWaterMark);
					#endif
*/

				}
			}
	}
}
